import sensor, image, time, pyb
from pyb import UART
from pyb import LED
from pyb import Pin


uart = UART(3, 9600)
red_led   = LED(1)
green_led   = LED(2)
blue_led   = LED(3)
pin6 = Pin('P6', Pin.IN, Pin.PULL_UP)

# 颜色阈值
red_threshold   = (14, 94, 20, 110, -42, 82) # 1
green_threshold =  (51, 78, -59, -28, -15, 50) # 2
blue_threshold = (26, 100, -55, 80, -49, -25) # 4

CheckColorState = 0
goal = 0
isOpen = 0; # 是否收到开始工作指令

sensor.reset() # 初始化sensor
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # 使用QQVGA的速度。
sensor.skip_frames(time = 1000) # 让新的设置生效。
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # 跟踪FPS帧率

# isOpen = 1
#goal = 1
while True:
    isBoard = pin6.value(); # 0有障碍物
    if uart.any():
        result = uart.read().decode()
        if result == 'O':
            isOpen = 1 # 开始工作 类似自锁

    if isOpen:
        # 识别到开启指令 不断搜寻目标值
        clock.tick() # 追踪两个snapshots()之间经过的毫秒数.
        img = sensor.snapshot() # 拍一张照片并返回图像。
        blobs = img.find_blobs([red_threshold,green_threshold,blue_threshold],roi=[33,86,20,50])
        if blobs and isBoard == 0:
            #如果找到了目标颜色
            for b in blobs:
                #迭代找到的目标颜色区域
                img.draw_rectangle(b[0:4]) # rect
                #用矩形标记出目标颜色区域
                img.draw_cross(b[5], b[6]) # cx, cy
                #在目标颜色区域的中心画十字形标记
                if b[8] == 1:
                    green_led.off()
                    blue_led.off()
                    red_led.on()
                if b[8] == 2:
                    red_led.off()
                    blue_led.off()
                    green_led.on()
                if b[8] == 4:
                    green_led.off()
                    red_led.off()
                    blue_led.on()
                if goal == 0:
                    # 目标值为0则使当前颜色代号做为目标值
                    goal = b[8] # 获得目标色卡
                    uart.write("G") # 识别到目标颜色 向主控发送前进信息
                    count = 0
                    while True:
                        # 等待离开此次色卡
                        if pin6.value() == 1:
                            count += 1
                        if count > 50:
                            break
                    break
                else:
                    if b[8] == goal:
                        count = 0
                        while True:
                            # 等待离开此次色卡
                            if pin6.value() == 1:
                                count += 1
                            if count > 10:
                                break
                        time.sleep_ms(390)
                        uart.write("S") # 找到目标颜色 向主控发送停止信息
                    break;
